
#ifndef __COMM_MAP_CPP__
#define __COMM_MAP_CPP__

#include "comm/comm_map.h"
#include "comm/comm_base.h"
#include "comm/comm_file.h"
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

SIMPLE_YAML_FILE_C::SIMPLE_YAML_FILE_C(const char *pcYamlFileName)
{
    if (pcYamlFileName == NULL) {
        ST_LOG_ERR("Str Is Null.");
        return;
    }

    strFileName_ = pcYamlFileName;
}

int SIMPLE_YAML_FILE_C::ReadItem(YAML_FILE_ITEM_STRU astItem[], int itemNum)
{
    /* 检查并设置初始值 */
    for (int loop = 0; loop < itemNum; loop++) {
        if (astItem[loop].pStrTag == NULL) {
            ST_LOG_ERR("Item Tag Is Null.");
            return -1;
        }

        astItem[loop].strContent[0] = '\0';
    }

    /* 从文件中读取 */
    if (0 != ReadItemFromFile(astItem, itemNum)) {
        return -1;
    }

    /* 检查读取结果 */
    for (int loop = 0; loop < itemNum; loop++) {
        if (astItem[loop].strContent[0] == '\0') {
            ST_LOG_ERR("Item(%s) Read Fail.", astItem[loop].pStrTag);
            return -1;
        }
    }
    return 0;
}

int SIMPLE_YAML_FILE_C::WriteItem(YAML_FILE_ITEM_STRU astItem[], int itemNum)
{
    FILE *pfFile = fopen(strFileName_.c_str(), "w");
    if (pfFile == NULL) {
        ST_LOG_ERR("Open File Fail.");
        return -1;
    }

    bool isWriteFirstLine = true;

    for (int loop = 0; loop < itemNum; loop++) {
        if (!isWriteFirstLine) {
            fprintf(pfFile, "\n");
        }

        YAML_FILE_ITEM_STRU *pstItem = &astItem[loop];
        fprintf(pfFile, "%s: %s", pstItem->pStrTag, pstItem->strContent);

        isWriteFirstLine = false;
    }

    fclose(pfFile);
    return 0;
}

void SIMPLE_YAML_FILE_C::DelAllCharInStr(char *pcStr, char delChar)
{
    int srcStrLen = strlen(pcStr);
    int tgtStrLen = 0;
    for (int loop = 0; loop < srcStrLen; loop++) {
        if (pcStr[loop] == delChar) {
            continue;
        }

        /* 可能不需要赋值的也执行了一遍，无影响 */
        pcStr[tgtStrLen] = pcStr[loop];
        tgtStrLen++;
    }

    pcStr[tgtStrLen] = '\0';
}

bool SIMPLE_YAML_FILE_C::IsStrBeginWith(const char *pcStr, const char *pcSubStr)
{
    int strLen = strlen(pcStr);
    int subStrLen = strlen(pcSubStr);
    if (strLen < subStrLen) {
        return false;
    }

    for (int loop = 0; loop < subStrLen; loop++) {
        if (pcStr[loop] != pcSubStr[loop]) {
            return false;
        }
    }

    return true;
}

YAML_FILE_ITEM_STRU *SIMPLE_YAML_FILE_C::MatchItem(YAML_FILE_ITEM_STRU astItem[], int itemNum, const char *pstrLine)
{
    for (int loop = 0; loop < itemNum; loop++) {
        YAML_FILE_ITEM_STRU *pstItemTmp = &astItem[loop];
        if (IsStrBeginWith(pstrLine, pstItemTmp->pStrTag) && pstrLine[strlen(pstItemTmp->pStrTag) == ':']) {
            return pstItemTmp;
        }
    }

    return NULL;
}

int SIMPLE_YAML_FILE_C::ReadItemFromFile(YAML_FILE_ITEM_STRU astItem[], int itemNum)
{
    FILE *pfRead = fopen(strFileName_.c_str(), "rb");
    if (pfRead == NULL) {
        ST_LOG_ERR("Open File(%s) Fail.", strFileName_.c_str());
        return -1;
    }

    char acLine[SIMPLE_YAML_FILE_MAX_ITEM_LEN];
    while (fgets(acLine, sizeof(acLine), pfRead) != NULL) {
        /* 确保有结束符 */
        acLine[sizeof(acLine) - 1] = '\0';

        /* 如果最后一个字符是换行，则删除换行 */
        //int lineLen = strlen(acLine);
        //if ((lineLen > 0) && (acLine[lineLen - 1] == '\n')) {
        //    acLine[lineLen - 1] = '\0';
        //}

        DelAllCharInStr(acLine, '\r');
        DelAllCharInStr(acLine, '\n');

        /* 删除空格方便处理 */
        DelAllCharInStr(acLine, ' ');
        DelAllCharInStr(acLine, '\t');

        YAML_FILE_ITEM_STRU *pstItem = MatchItem(astItem, itemNum, acLine);
        if (pstItem == NULL) {
            continue;
        }

        /* 跳过:赋值剩下的内容 */
        strncpy(pstItem->strContent, &acLine[strlen(pstItem->pStrTag) + 1], sizeof(pstItem->strContent) - 1);
        pstItem->strContent[sizeof(pstItem->strContent) - 1] = '\0';
    }

    fclose(pfRead);
    return 0;
}

int MAP_YAML_FILE_C::Read(const string &strMapPath, string &strPgmFileName, float &resolution, COORDINATE_2D_STRU &stOriginPos) const
{
    YAML_FILE_ITEM_STRU *pstItem;
    YAML_FILE_ITEM_STRU astItem[] = {{"image",      {0}},
                                     {"resolution", {0}},
                                     {"origin",     {0}}};      /* 此处顺序和后续处理顺序要一致 */

    /* 从文件中读取各项 */
    string strYamlFilePath;
    GetYamlFilePath(strMapPath, strYamlFilePath);
    SIMPLE_YAML_FILE_C objYamlFile(strYamlFilePath.c_str());
    if (0 != objYamlFile.ReadItem(astItem, ARRAY_SIZE(astItem))) {
        ST_LOG_ERR("Read Content Fail.");
        return -1;
    }

    /* 处理 image  */
    pstItem = &astItem[0];
    strPgmFileName.assign(pstItem->strContent);
    ST_LOG_INFO("image Content(%s), pcPgmFileName=%s.", pstItem->strContent, strPgmFileName.c_str());

    /* 处理 resolution  */
    pstItem = &astItem[1];
    if (1 != sscanf(pstItem->strContent, "%f", &resolution)) {
        ST_LOG_ERR("resolution Content(%s) Trans Fail.", pstItem->strContent);
        return -1;
    }
    ST_LOG_INFO("resolution Content(%s), resolution=%f.", pstItem->strContent, resolution);

    /* 处理 origin  */
    pstItem = &astItem[2];
    float originPosZ;
    if (3 != sscanf(pstItem->strContent, "[%f,%f,%f]", &stOriginPos.x, &stOriginPos.y, &originPosZ)) {
        ST_LOG_ERR("origin Content(%s) Trans Fail.", pstItem->strContent);
        return -1;
    }
    ST_LOG_INFO("origin Content(%s), x=%f, y=%f.", pstItem->strContent, stOriginPos.x, stOriginPos.y);

    return 0;
}

int MAP_YAML_FILE_C::Write(const string &strMapPath, float resolution, const COORDINATE_2D_STRU &stOriginPos) const
{
    string strYamlFilePath;
    GetYamlFilePath(strMapPath, strYamlFilePath);

    string strPgmFileName;
    GetPgmFileName(strMapPath, strPgmFileName);

    SIMPLE_YAML_FILE_C objYamlFile(strYamlFilePath.c_str());
    YAML_FILE_ITEM_STRU astItem[6];
    astItem[0].pStrTag = "image";
    sprintf(astItem[0].strContent, "%s", strPgmFileName.c_str());
    astItem[1].pStrTag = "resolution";
    sprintf(astItem[1].strContent, "%.6f", resolution);
    astItem[2].pStrTag = "origin";
    sprintf(astItem[2].strContent, "[%.6f, %.6f, 0.000000]", stOriginPos.x, stOriginPos.y);
    astItem[3].pStrTag = "negate";
    sprintf(astItem[3].strContent, "0");
    astItem[4].pStrTag = "occupied_thresh";
    sprintf(astItem[4].strContent, "0.65");
    astItem[5].pStrTag = "free_thresh";
    sprintf(astItem[5].strContent, "0.196");
    if (0 != objYamlFile.WriteItem(astItem, ARRAY_SIZE(astItem))) {
        ST_LOG_ERR("Write to file fail.");
        return -1;
    }

    return 0;
}

void MAP_YAML_FILE_C::GetYamlFilePath(const string &strMapPath, string &strYamlFilePath) const
{
    strYamlFilePath = strMapPath + ".yaml";
}

void MAP_YAML_FILE_C::GetPgmFileName(const string &strMapPath, string &strPgmFileName) const
{
    string strMapName;

    FILE_OPRT_C objFileOpr;
    objFileOpr.GetFileNameFrmPath(strMapPath, strMapName);

    strPgmFileName = strMapName + ".pgm";
}

PGM_FILE_C::PGM_FILE_C(void)
{
}

PGM_FILE_C::~PGM_FILE_C(void)
{
}

int PGM_FILE_C::ReadImageSize(const string &strMapPath, unsigned int &imageCols, unsigned int &imageRows)
{
    string strPgmFilePath;
    GetPgmFilePath(strMapPath, strPgmFilePath);

    FILE *pfRead = fopen(strPgmFilePath.c_str(), "rb");
    if (pfRead == NULL) {
        return -1;
    }

    if (0 != ReadFileHead(pfRead, imageCols, imageRows)) {
        fclose(pfRead);
        return -1;
    }

    fclose(pfRead);
    return 0;
}

int PGM_FILE_C::ReadImage(const string &strMapPath, unsigned char *pucImage, unsigned int imageMaxSize)
{
    string strPgmFilePath;
    GetPgmFilePath(strMapPath, strPgmFilePath);

    FILE *pfRead = fopen(strPgmFilePath.c_str(), "rb");
    if (pfRead == NULL) {
        ST_LOG_ERR("Open %s Fail.", strMapPath.c_str());
        return -1;
    }

    unsigned int imageCols;
    unsigned int imageRows;
    if (   (0 != ReadFileHead(pfRead, imageCols, imageRows))
        || (imageMaxSize != (imageCols * imageRows))) {
        ST_LOG_ERR("Image Size(%u %u) Not Equ MaxSize(%u).", imageCols, imageRows, imageMaxSize);
        fclose(pfRead);
        return -1;
    }

    if (imageMaxSize != fread(pucImage, 1, imageMaxSize, pfRead)) {
        ST_LOG_ERR("Read Fail.");
        fclose(pfRead);
        return -1;
    }

    fclose(pfRead);
    return 0;
}

void PGM_FILE_C::SaveFile(const string &strMapPath, unsigned char *pucImage, unsigned int imageCols, unsigned int imageRows)
{
    string strPgmFilePath;
    GetPgmFilePath(strMapPath, strPgmFilePath);

    char acFileHead[100] = {0};
    sprintf(acFileHead, "P5\n%d %d\n255\n", imageCols, imageRows);

    FILE *pfSave = fopen(strPgmFilePath.c_str(), "wb");
    if (pfSave == NULL) {
        return;
    }

    (void)fwrite(acFileHead, 1, strlen(acFileHead), pfSave);
    (void)fwrite(pucImage, 1, imageCols * imageRows, pfSave);

    fclose(pfSave);
}

void PGM_FILE_C::GetPgmFilePath(const string &strMapPath, string &strPgmFilePath)
{
    strPgmFilePath = strMapPath + ".pgm";
}

int PGM_FILE_C::ReadNextLine(FILE *pfRead, char *pucLine, int maxLineLen)
{
    char *pcRslt;

    do {
        pcRslt = fgets(pucLine, maxLineLen, pfRead);
        /* # 之后的为解释文本，跳过 */
    } while ((pcRslt != NULL) && (*pcRslt == '#'));

    return (pcRslt == NULL) ? -1 : 0;
}

int PGM_FILE_C::ReadFileHead(FILE *pfRead, unsigned int &imageCols, unsigned int &imageRows)
{
    char acLine[1024];      /* 假设解释信息最长1024字节 */
    unsigned int imageMaxVal;

    if (   (0 != ReadNextLine(pfRead, acLine, sizeof(acLine)))
        || (acLine[0] != 'P') || (acLine[1] != '5')) {
        return -1;
    }

    if (   (0 != ReadNextLine(pfRead, acLine, sizeof(acLine)))
        || (2 != sscanf(acLine, "%u %u", &imageCols, &imageRows))) {
        return -1;
    }

    if (   (0 != ReadNextLine(pfRead, acLine, sizeof(acLine)))
        || (1 != sscanf(acLine, "%u", &imageMaxVal))
        || (imageMaxVal > 255)) {
        return -1;
    }

    return 0;
}

GRAY_IMAGE_C::GRAY_IMAGE_C(void)
{
    pucMap_ = NULL;
    mapSizeX_ = 0;
    mapSizeY_ = 0;
}

GRAY_IMAGE_C::~GRAY_IMAGE_C(void)
{
    FreeImage();
}

/**************************************************************************************
功能描述: 创建图像
修改记录:
**************************************************************************************/
INT32 GRAY_IMAGE_C::CreateImage(UINT32 sizeX, UINT32 sizeY)
{
    FreeImage();

    mapSizeX_ = sizeX;
    mapSizeY_ = sizeY;

    pucMap_ = new unsigned char[mapSizeX_ * mapSizeY_];
    if (pucMap_ == NULL) {
        ST_LOG_ERR("Alloc Mem Fail.");
        return -1;
    }

    //ST_LOG_INFO("Create Image Succ. SizeX=%u, SizeY=%u.", mapSizeX_, mapSizeY_);
    return 0;
}


/**************************************************************************************
功能描述: 创建图像
修改记录:
**************************************************************************************/
INT32 GRAY_IMAGE_C::CreateImage(UINT32 sizeX, UINT32 sizeY, UINT08 defaultVal)
{
    if (0 != CreateImage(sizeX, sizeY)) {
        return -1;
    }

    memset(pucMap_, defaultVal, mapSizeX_ * mapSizeY_);

    //ST_LOG_INFO("CreateImage Succ. SizeX=%u, SizeY=%u.", mapSizeX_, mapSizeY_);
    return 0;
}

/**************************************************************************************
功能描述: 创建图像
修改记录:
**************************************************************************************/
INT32 GRAY_IMAGE_C::CreateImage(UINT32 sizeX, UINT32 sizeY, UINT08 *pucImage)
{
    R_ASSERT(pucImage != NULL, -1);

    if (0 != CreateImage(sizeX, sizeY)) {
        ST_LOG_ERR("Create Fail.");
        return -1;
    }

    /* 原始图像要上下颠倒 */
    TurnMapUpsideDown(pucImage, pucMap_);

    //ST_LOG_INFO("CreateImage Succ. SizeX=%u, SizeY=%u.", mapSizeX_, mapSizeY_);
    return 0;
}

/**************************************************************************************
功能描述: 重置图像范围
修改记录:
**************************************************************************************/
INT32 GRAY_IMAGE_C::ResizeImage(const IMAGE_COORDINATE_STRU &stNewMapSize, const IMAGE_COORDINATE_STRU &stNewMapLeftButtCoordByOld, UINT08 defaultVal)
{
    R_ASSERT(IsImageValid(), -1);
    R_ASSERT((stNewMapSize.x > 0) && (stNewMapSize.y > 0), -1);

    UINT08 *pucNewMap = (UINT08 *)new UINT08[stNewMapSize.x * stNewMapSize.y];
    if (pucNewMap == NULL) {
        ST_LOG_INFO("Alloc New Map Fail.");
        return -1;
    }
    memset(pucNewMap, defaultVal, stNewMapSize.x * stNewMapSize.y);

    /* 找公共部分 */
    IMAGE_COORDINATE_STRU stNewMapRightTopCoordByOld;
    stNewMapRightTopCoordByOld.x = stNewMapLeftButtCoordByOld.x + stNewMapSize.x - 1;
    stNewMapRightTopCoordByOld.y = stNewMapLeftButtCoordByOld.y + stNewMapSize.y - 1;

    IMAGE_COORDINATE_STRU stCommLeftBuffCoordByOld;
    stCommLeftBuffCoordByOld.x = MAX(0, stNewMapLeftButtCoordByOld.x);
    stCommLeftBuffCoordByOld.y = MAX(0, stNewMapLeftButtCoordByOld.y);

    IMAGE_COORDINATE_STRU stCommRightTopCoordByOld;
    stCommRightTopCoordByOld.x = MIN(INT32(mapSizeX_ - 1), stNewMapRightTopCoordByOld.x);
    stCommRightTopCoordByOld.y = MIN(INT32(mapSizeY_ - 1), stNewMapRightTopCoordByOld.y);

    /* 如果有公共部分，复制到新地图 */
    if ((stCommLeftBuffCoordByOld.x <= stCommRightTopCoordByOld.x) && (stCommLeftBuffCoordByOld.y <= stCommRightTopCoordByOld.y)) {
        IMAGE_COORDINATE_STRU stCommSize;
        stCommSize.x = stCommRightTopCoordByOld.x - stCommLeftBuffCoordByOld.x + 1;
        stCommSize.y = stCommRightTopCoordByOld.y - stCommLeftBuffCoordByOld.y + 1;

        IMAGE_COORDINATE_STRU stCommLeftButtCoordByNew;
        stCommLeftButtCoordByNew.x = stCommLeftBuffCoordByOld.x - stNewMapLeftButtCoordByOld.x;
        stCommLeftButtCoordByNew.y = stCommLeftBuffCoordByOld.y - stNewMapLeftButtCoordByOld.y;

        for (INT32 loop = 0; loop < stCommSize.y; ++loop) {
            UINT08 *pucOldLine = &pucMap_[(stCommLeftBuffCoordByOld.y + loop) * mapSizeX_ + stCommLeftBuffCoordByOld.x];
            UINT08 *pucNewLine = &pucNewMap[(stCommLeftButtCoordByNew.y + loop) * stNewMapSize.x + stCommLeftButtCoordByNew.x];
            memcpy(pucNewLine, pucOldLine, stCommSize.x);
        }
    }

    /* 更新老参数 */
    delete[] pucMap_;
    pucMap_   = pucNewMap;
    mapSizeX_ = stNewMapSize.x;
    mapSizeY_ = stNewMapSize.y;

    return 0;
}

/**************************************************************************************
功能描述: 从现有图像中拷贝一部分到本图像
修改记录:
**************************************************************************************/
INT32 GRAY_IMAGE_C::CopyImage(const GRAY_IMAGE_C &objSrcImage, const IMAGE_COORDINATE_STRU &stSrcLeftButtCoord, const IMAGE_COORDINATE_STRU &stCopySize)
{
    R_ASSERT(objSrcImage.IsImageValid(), -1);

    IMAGE_COORDINATE_STRU stSrcRightTopCoord;
    stSrcRightTopCoord.x = stSrcLeftButtCoord.x + stCopySize.x - 1;
    stSrcRightTopCoord.y = stSrcLeftButtCoord.y + stCopySize.y - 1;
    if (!objSrcImage.isCoordValid(stSrcLeftButtCoord) || !objSrcImage.isCoordValid(stSrcRightTopCoord) || (stCopySize.x <= 0) || (stCopySize.y <= 0)) {
        ST_LOG_ERR("Src Rang Coord Is Invalid.");
        return -1;
    }

    INT32 rslt = CreateImage(stCopySize.x, stCopySize.y);
    if (rslt != 0) {
        ST_LOG_ERR("CreateImage() Fail.");
        return -1;
    }

    for (INT32 loop = 0; loop < stCopySize.y; loop++) {
        memcpy(&pucMap_[loop * stCopySize.x], &objSrcImage.pucMap_[(stSrcLeftButtCoord.y + loop) * objSrcImage.mapSizeX_+ stSrcLeftButtCoord.x], stCopySize.x);
    }

    mapSizeX_ = stCopySize.x;
    mapSizeY_ = stCopySize.y;
    return 0;
}

INT32 GRAY_IMAGE_C::CopyImage(const GRAY_IMAGE_C &objSrcImage)
{
    R_ASSERT(objSrcImage.IsImageValid(), -1);

    INT32 rslt = CreateImage(objSrcImage.mapSizeX_, objSrcImage.mapSizeY_);
    if (rslt != 0) {
        ST_LOG_ERR("CreateImage() Fail.");
        return -1;
    }

    mapSizeX_ = objSrcImage.mapSizeX_;
    mapSizeY_ = objSrcImage.mapSizeY_;
    memcpy(pucMap_, objSrcImage.pucMap_, mapSizeX_ * mapSizeY_);
    return 0;
}

/**************************************************************************************
功能描述: 从pgm中加载地图
修改记录:
**************************************************************************************/
INT32 GRAY_IMAGE_C::LoadImage(const string &strMapPath)
{
    FreeImage();

    PGM_FILE_C objPgmFile;
    if (0 != objPgmFile.ReadImageSize(strMapPath, mapSizeX_, mapSizeY_)) {
        ST_LOG_ERR("Read Pgm Size Fail.");
        return -1;
    }

    UINT32 mapSize =  mapSizeX_ * mapSizeY_;

    unsigned char *pucMapTmp = new unsigned char[mapSize];
    if (pucMapTmp == NULL) {
        ST_LOG_ERR("Alloc Tmp Map Fail.");
        return -1;
    }

    pucMap_ = new unsigned char[mapSize];
    if (pucMap_ == NULL) {
        ST_LOG_ERR("Alloc Map Fail.");
        delete[] pucMapTmp;
        return -1;
    }

    if (0 != objPgmFile.ReadImage(strMapPath, pucMapTmp, mapSize)) {
        delete[] pucMapTmp;
        delete[] pucMap_;
        pucMap_ = NULL;
        ST_LOG_ERR("Read Pgm Image Fail.");
        return -1;
    }

    /* 由于图像坐标系和地图坐标系Y轴时反的，所以要将图片中行上下颠倒 */
    TurnMapUpsideDown(pucMapTmp, pucMap_);
    delete[] pucMapTmp;

    ST_LOG_INFO("Load %s Succ.", strMapPath.c_str());
    return 0;
}

INT32 GRAY_IMAGE_C::SaveImage(const string &strMapPath) const
{
    R_ASSERT(pucMap_ != NULL, -1);

    /* 写PGM文件 */
    /* 地图读取的时候上下反过来了，还要反回去 */
    unsigned char *pucMapTmp = new unsigned char[mapSizeX_ * mapSizeY_];
    if (pucMapTmp == NULL) {
        ST_LOG_ERR("Alloc Tmp Map Fail.");
        return -1;
    }

    TurnMapUpsideDown(pucMap_, pucMapTmp);

    PGM_FILE_C objPgmFile;
    objPgmFile.SaveFile(strMapPath, pucMapTmp, mapSizeX_, mapSizeY_);
    delete[] pucMapTmp;

    return 0;
}

void GRAY_IMAGE_C::FreeImage(void)
{
    delete[] pucMap_;
    pucMap_ = NULL;
}

void GRAY_IMAGE_C::SwapImage(GRAY_IMAGE_C &objOtherImage)
{
    UINT32  otherImageSizeX = objOtherImage.mapSizeX_;
    UINT32  otherImageSizeY = objOtherImage.mapSizeY_;
    UINT08 *pucOtherMap     = objOtherImage.pucMap_;

    objOtherImage.mapSizeX_ = this->mapSizeX_;
    objOtherImage.mapSizeY_ = this->mapSizeY_;
    objOtherImage.pucMap_   = this->pucMap_;

    this->mapSizeX_ = otherImageSizeX;
    this->mapSizeY_ = otherImageSizeY;
    this->pucMap_   = pucOtherMap;
}

bool GRAY_IMAGE_C::IsImageValid(void) const
{
    return ((pucMap_ != NULL) && (mapSizeX_ != 0) && (mapSizeY_ != 0));
}

bool GRAY_IMAGE_C::isCoordValid(int cellX, int cellY) const
{
    if ((pucMap_ == NULL) || (cellX < 0) || (cellX >= (int)mapSizeX_) || (cellY < 0) || (cellY >= (int)mapSizeY_)) {
        return false;
    } else {
        return true;
    }
}

bool GRAY_IMAGE_C::isCoordValid(const IMAGE_COORDINATE_STRU &stCoord) const
{
    return isCoordValid(stCoord.x, stCoord.y);
}

INT32 GRAY_IMAGE_C::GetImageSize(UINT32 &sizeX, UINT32 &sizeY) const
{
    R_ASSERT(IsImageValid(), -1);

    sizeX = mapSizeX_;
    sizeY = mapSizeY_;
    return 0;
}

UINT32 GRAY_IMAGE_C::GetImageSizeX(void) const
{
    return mapSizeX_;
}

UINT32 GRAY_IMAGE_C::GetImageSizeY(void) const
{
    return mapSizeY_;
}

void GRAY_IMAGE_C::GetLBCoord(IMAGE_COORDINATE_STRU &stLBCoord) const
{
    stLBCoord.x = 0;
    stLBCoord.y = 0;
}

void GRAY_IMAGE_C::GetRTCoord(IMAGE_COORDINATE_STRU &stRTCoord) const
{
    stRTCoord.x = mapSizeX_ - 1;
    stRTCoord.y = mapSizeY_ - 1;
}

INT32 GRAY_IMAGE_C::TransSrcImageCoordToLoc(const IMAGE_COORDINATE_STRU &stSrcImageCoord, IMAGE_COORDINATE_STRU &stLocImageCoord)
{
    if (!isCoordValid(stSrcImageCoord)) {
        return -1;
    }

    stLocImageCoord.x = stSrcImageCoord.x;
    stLocImageCoord.y = ((int)mapSizeY_ - 1) - stSrcImageCoord.y;
    return 0;
}

void GRAY_IMAGE_C::SetImage(unsigned char setVal)
{
    if (pucMap_ == NULL) {
        return;
    }

    memset(pucMap_, setVal, mapSizeX_ * mapSizeY_);
}

void GRAY_IMAGE_C::SetImage(const IMAGE_COORDINATE_STRU &stAreaLeftButtCoord, UINT32 areaSizeX, UINT32 areaSizeY, UINT08 *pucMapArea)
{
    IMAGE_COORDINATE_STRU stAreaRightTopCoord;
    stAreaRightTopCoord.x = stAreaLeftButtCoord.x + areaSizeX - 1;
    stAreaRightTopCoord.y = stAreaLeftButtCoord.y + areaSizeY - 1;

    R_ASSERT(IsImageValid(), NOTHING);
    R_ASSERT(isCoordValid(stAreaLeftButtCoord), NOTHING);
    R_ASSERT(isCoordValid(stAreaRightTopCoord), NOTHING);

    for (INT32 loop = 0; loop < INT32(areaSizeY); ++loop) {
        memcpy(&pucMap_[(stAreaLeftButtCoord.y + loop) * mapSizeX_ + stAreaLeftButtCoord.x],
               &pucMapArea[loop * areaSizeX],
               areaSizeX);
    }
}

void GRAY_IMAGE_C::SetCell(int coordX, int coordY, unsigned char setVal)
{
    if (!isCoordValid(coordX, coordY)) {
        return;
    }
    pucMap_[coordY * mapSizeX_ + coordX] = setVal;
}

void GRAY_IMAGE_C::SetCell(IMAGE_COORDINATE_STRU *pstCoord, unsigned char setVal)
{
    SetCell(pstCoord->x, pstCoord->y, setVal);
}

void GRAY_IMAGE_C::SetAreaOfSameColor(const IMAGE_COORDINATE_STRU &stRefPixel, UINT08 newColor)
{
    UINT08 refColor;
    INT32 rslt = GetCell(stRefPixel.x, stRefPixel.y, refColor);
    R_ASSERT_2(rslt == 0, NOTHING, stRefPixel.x, stRefPixel.y);
    R_ASSERT_2(refColor != newColor, NOTHING, refColor, newColor);
#if 0
    list<IMAGE_COORDINATE_STRU> listToProcPixel;
    SetCell(stRefPixel.x, stRefPixel.y, newColor);
    listToProcPixel.push_back(stRefPixel);

    while (!listToProcPixel.empty()) {
        const IMAGE_COORDINATE_STRU &stFirstPixel = listToProcPixel.front();

        IMAGE_COORDINATE_STRU astAroundPixel[] = {{stFirstPixel.x,     stFirstPixel.y + 1},
                                                  {stFirstPixel.x,     stFirstPixel.y - 1},
                                                  {stFirstPixel.x - 1, stFirstPixel.y},
                                                  {stFirstPixel.x + 1, stFirstPixel.y}};
        for (UINT32 loop = 0; loop < ARRAY_SIZE(astAroundPixel); ++loop) {
            const IMAGE_COORDINATE_STRU &stAroundPixel = astAroundPixel[loop];

            UINT08 tmpColor;
            rslt = GetCell(stAroundPixel.x, stAroundPixel.y, tmpColor);
            if ((rslt == 0) && (tmpColor == refColor)) {
                SetCell(stAroundPixel.x, stAroundPixel.y, newColor);
                listToProcPixel.push_back(stAroundPixel);
            }
        }

        listToProcPixel.pop_front();
    }
#else
    /* 这种方法比上面的方法效率高一倍左右 */
    vector<IMAGE_COORDINATE_STRU> vectToPorcPixel;
    vectToPorcPixel.reserve(mapSizeX_ * mapSizeY_);

    SetCell(stRefPixel.x, stRefPixel.y, newColor);
    vectToPorcPixel.push_back(stRefPixel);

    for (UINT32 loop = 0; loop < vectToPorcPixel.size(); ++loop) {
        const IMAGE_COORDINATE_STRU &stProcPixel = vectToPorcPixel[loop];

        IMAGE_COORDINATE_STRU astAroundPixel[] = {{stProcPixel.x,     stProcPixel.y + 1},
                                                  {stProcPixel.x,     stProcPixel.y - 1},
                                                  {stProcPixel.x - 1, stProcPixel.y},
                                                  {stProcPixel.x + 1, stProcPixel.y}};      /* 当前处理像素的上下左右4个 */
        for (UINT32 loopOfAround = 0; loopOfAround < ARRAY_SIZE(astAroundPixel); ++loopOfAround) {
            const IMAGE_COORDINATE_STRU &stAroundPixel = astAroundPixel[loopOfAround];

            UINT08 tmpColor;
            rslt = GetCell(stAroundPixel.x, stAroundPixel.y, tmpColor);
            if ((rslt == 0) && (tmpColor == refColor)) {
                SetCell(stAroundPixel.x, stAroundPixel.y, newColor);
                vectToPorcPixel.push_back(stAroundPixel);
            }
        }
    }
#endif
}

/**************************************************************************************
功能描述: 设置指定曲线的左右两侧像素
修改记录:
**************************************************************************************/
INT32 GRAY_IMAGE_C::SetLeftAndRightPixelsOfLine(const vector<IMAGE_COORDINATE_STRU> &vectLine, UINT08 setLeftVal, UINT08 setRightVal, UINT08 splitVal, UINT32 maxStepLen)
{
    UINT32 lineSize = UINT32(vectLine.size());
    R_ASSERT_1(lineSize > 1, -1, lineSize);

    vector<IMAGE_COORDINATE_STRU> vectLeftPixel;
    vector<IMAGE_COORDINATE_STRU> vectRightPixel;

    vectLeftPixel.reserve(lineSize * 4);
    vectRightPixel.reserve(lineSize * 4);

    /* 第一步，找到指定曲线两侧紧挨着的种子像素 */
    for (UINT32 loop = 1; loop < lineSize; ++loop) {
        const IMAGE_COORDINATE_STRU &stCurPixel  = vectLine[loop - 1];
        const IMAGE_COORDINATE_STRU &stNextPixel = vectLine[loop];

        IMAGE_COORDINATE_STRU stNextPixelRelPos;
        stNextPixelRelPos.x = stNextPixel.x - stCurPixel.x;
        stNextPixelRelPos.y = stNextPixel.y - stCurPixel.y;

        #define PROC_AROUND_PIXEL_SIDE(xOfRelPos_, yOfRelPos_)                                                                      \
            do {                                                                                                                    \
                IMAGE_COORDINATE_STRU stAroundPixel;                                                                                \
                stAroundPixel.x = stCurPixel.x + (xOfRelPos_);                                                                      \
                stAroundPixel.y = stCurPixel.y + (yOfRelPos_);                                                                      \
                UINT08 *pucPixel = GetCellPtr(stAroundPixel.x, stAroundPixel.y);                                                    \
                if ((pucPixel != NULL) && (*pucPixel != setLeftVal) && (*pucPixel != setRightVal) && (*pucPixel != splitVal)) {     \
                    float crossMultiVal = stNextPixelRelPos.x * (yOfRelPos_) - stNextPixelRelPos.y * (xOfRelPos_);                  \
                    if (IS_FLOAT_ALMOST_EQUAL_ZERO(crossMultiVal)) {                                                                \
                        /* 线上的不要 */                                                                                            \
                    } else if (crossMultiVal > 0) {                                                                                 \
                        vectLeftPixel.push_back(stAroundPixel);                                                                     \
                        *pucPixel = setLeftVal;                                                                                     \
                    } else {                                                                                                        \
                        vectRightPixel.push_back(stAroundPixel);                                                                    \
                        *pucPixel = setRightVal;                                                                                    \
                    }                                                                                                               \
                }                                                                                                                   \
            } while (0)

        PROC_AROUND_PIXEL_SIDE( 0,  1);
        PROC_AROUND_PIXEL_SIDE( 0, -1);
        PROC_AROUND_PIXEL_SIDE(-1,  0);
        PROC_AROUND_PIXEL_SIDE( 1,  0);
    }

    /* 第二步，根据种子像素，向四周扩散指定步长 */
    for (UINT32 loop = 0; loop <= maxStepLen; ++loop) {
        vector<IMAGE_COORDINATE_STRU> vectNextStepRightPixel;
        vector<IMAGE_COORDINATE_STRU> vectNextStepLeftPixel;

        UINT32 rightPixelSize = vectRightPixel.size();
        UINT32 leftPixelSize  = vectLeftPixel.size();

        vectNextStepRightPixel.reserve(rightPixelSize * 4);
        vectNextStepLeftPixel.reserve(leftPixelSize * 4);

        for (UINT32 loopOfPixel = 0; loopOfPixel < rightPixelSize; ++loopOfPixel) {
            const IMAGE_COORDINATE_STRU &stPixel = vectRightPixel[loopOfPixel];

            #define PROC_RIGHT_PIXEL(xOfRelPos_, yOfRelPos_)                                                                        \
                do {                                                                                                                \
                    IMAGE_COORDINATE_STRU stAroundPixel;                                                                            \
                    stAroundPixel.x = stPixel.x + (xOfRelPos_);                                                                     \
                    stAroundPixel.y = stPixel.y + (yOfRelPos_);                                                                     \
                    UINT08 *pucPixel = GetCellPtr(stAroundPixel.x, stAroundPixel.y);                                                \
                    if ((pucPixel != NULL) && (*pucPixel != setLeftVal) && (*pucPixel != setRightVal) && (*pucPixel != splitVal)) { \
                        vectNextStepRightPixel.push_back(stAroundPixel);                                                            \
                        *pucPixel = setRightVal;                                                                                    \
                    }                                                                                                               \
                } while (0)

            PROC_RIGHT_PIXEL( 0,  1);
            PROC_RIGHT_PIXEL( 0, -1);
            PROC_RIGHT_PIXEL(-1,  0);
            PROC_RIGHT_PIXEL( 1,  0);
        }

        for (UINT32 loopOfPixel = 0; loopOfPixel < leftPixelSize; ++loopOfPixel) {
            const IMAGE_COORDINATE_STRU &stPixel = vectLeftPixel[loopOfPixel];

            #define PROC_LEFT_PIXEL(xOfRelPos_, yOfRelPos_)                                                                         \
                do {                                                                                                                \
                    IMAGE_COORDINATE_STRU stAroundPixel;                                                                            \
                    stAroundPixel.x = stPixel.x + (xOfRelPos_);                                                                     \
                    stAroundPixel.y = stPixel.y + (yOfRelPos_);                                                                     \
                    UINT08 *pucPixel = GetCellPtr(stAroundPixel.x, stAroundPixel.y);                                                \
                    if ((pucPixel != NULL) && (*pucPixel != setLeftVal) && (*pucPixel != setRightVal) && (*pucPixel != splitVal)) { \
                        vectNextStepLeftPixel.push_back(stAroundPixel);                                                             \
                        *pucPixel = setLeftVal;                                                                                     \
                    }                                                                                                               \
                } while (0)

            PROC_LEFT_PIXEL( 0,  1);
            PROC_LEFT_PIXEL( 0, -1);
            PROC_LEFT_PIXEL(-1,  0);
            PROC_LEFT_PIXEL( 1,  0);
        }

        vectNextStepRightPixel.swap(vectRightPixel);
        vectNextStepLeftPixel.swap(vectLeftPixel);
    }

    return 0;
}

INT32 GRAY_IMAGE_C::GetCell(int cellX, int cellY, UINT08 &cellVal) const
{
    if (!isCoordValid(cellX, cellY)) {
        return -1;
    }

    cellVal = pucMap_[cellY * mapSizeX_ + cellX];
    return 0;
}

/**************************************************************************************
功能描述: 获取指定位置的地图的指针
修改记录:
**************************************************************************************/
UINT08 *GRAY_IMAGE_C::GetCellPtr(int cellX, int cellY) const
{
    if (!isCoordValid(cellX, cellY)) {
        return NULL;
    }

    return &pucMap_[cellY * mapSizeX_ + cellX];
}

INT32 GRAY_IMAGE_C::GetCellsInArea(UINT08 cellVal, const IMAGE_COORDINATE_STRU &stAreaLBCoord, const IMAGE_COORDINATE_STRU &stAreaSize, vector<IMAGE_COORDINATE_STRU> &vectCellList) const
{
    R_ASSERT(IsImageValid(), -1);

    IMAGE_COORDINATE_STRU stCoord;
    for (INT32 loopX = 0; loopX < stAreaSize.x; ++loopX) {
        stCoord.x = stAreaLBCoord.x + loopX;

        for (INT32 loopY = 0; loopY < stAreaSize.y; ++loopY) {
            stCoord.y = stAreaLBCoord.y + loopY;

            if (cellVal == pucMap_[stCoord.y * mapSizeX_ + stCoord.x]) {
                vectCellList.push_back(stCoord);
            }
        }
    }

    return 0;
}

void GRAY_IMAGE_C::TurnMapUpsideDown(unsigned char *pucSrcMap, unsigned char *pucTgtMap) const
{
    for (unsigned int loop = 0; loop < mapSizeY_; loop++) {
        memcpy(&pucTgtMap[(mapSizeY_ - loop - 1) * mapSizeX_],
               &pucSrcMap[loop * mapSizeX_],
               mapSizeX_);
    }
}

GRAY_MAP_C::GRAY_MAP_C(void)
{
    stOriginPos_.x   = 0;
    stOriginPos_.y   = 0;
    imageResolution_ = MAP_RESOLUTION;
}

GRAY_MAP_C::~GRAY_MAP_C(void)
{
}

BOOL GRAY_MAP_C::IsValid(void) const
{
    return IsImageValid();
}

/* 该函数为定制使用，其它地方不要使用 */
int GRAY_MAP_C::GetMapInfo(const string &strMapPath, string &strPgmFileNameFrmYaml, IMAGE_COORDINATE_STRU &stMapSize, COORDINATE_2D_STRU &stOriginPos)
{
    /* 从YAML文件中读取原点坐标 */
    float  mapResolution;
    MAP_YAML_FILE_C objYamlFile;
    if (0 != objYamlFile.Read(strMapPath, strPgmFileNameFrmYaml, mapResolution, stOriginPos)) {
        ST_LOG_ERR("Read Yaml File Fail.");
        return -1;
    }

    /* 从PGM文件中读取地图大小 *//* 注意，pgm文件名不要从yaml中度，直接改变文件扩展名即可。 */
    UINT32   sizeX;
    UINT32   sizeY;
    PGM_FILE_C objPgmFile;
    if (0 != objPgmFile.ReadImageSize(strMapPath, sizeX, sizeY)) {
        ST_LOG_ERR("Read Pgm Size Fail.");
        return -1;
    }
    stMapSize.x = (int)sizeX;
    stMapSize.y = (int)sizeY;

    return 0;
}

int GRAY_MAP_C::GetMapInfo(COORDINATE_2D_STRU &stOriginPos, float &resolution, UINT32 &sizeX, UINT32 &sizeY, UINT08 **ppucMap) const
{
    R_ASSERT(IsImageValid(), -1);
    R_ASSERT(ppucMap != NULL, -1);

    stOriginPos.Copy(stOriginPos_);
    resolution = imageResolution_;
    sizeX      = GetImageSizeX();
    sizeY      = GetImageSizeY();
    *ppucMap   = GetCellPtr(0, 0);

    return 0;
}

void GRAY_MAP_C::GetMapSize(IMAGE_COORDINATE_STRU &stSize) const
{
    stSize.x = 0;
    stSize.y = 0;

    R_ASSERT(IsImageValid(), NOTHING);

    stSize.x = INT32(GetImageSizeX());
    stSize.y = INT32(GetImageSizeY());
}

void GRAY_MAP_C::GetMapSize(COORDINATE_2D_STRU &stSize) const
{
    IMAGE_COORDINATE_STRU stImageSize;
    GetMapSize(stImageSize);

    stSize.x = imageResolution_ * float(stImageSize.x);
    stSize.y = imageResolution_ * float(stImageSize.y);
}

INT32 GRAY_MAP_C::GetMapSize(void) const
{
    IMAGE_COORDINATE_STRU stImageSize;
    GetMapSize(stImageSize);

    return stImageSize.x * stImageSize.y;
}

/**************************************************************************************
功能描述:  将图片位置转换为地图位置
修改记录:
**************************************************************************************/
int GRAY_MAP_C::TransPictureCoordToPos(IMAGE_COORDINATE_STRU &stPictureCoord, COORDINATE_2D_STRU &stMapPos)
{
    IMAGE_COORDINATE_STRU stMapCoord;

    if (0 != TransSrcImageCoordToLoc(stPictureCoord, stMapCoord)) {
        return -1;
    }

    ConvMapCoordToPos(stMapCoord, stMapPos);
    return 0;
}

UINT08 GRAY_MAP_C::GetCostOfObst(void) const
{
    return cellCostObst_;
}

UINT08 GRAY_MAP_C::GetCostOfNull(void) const
{
    return cellCostNull_;
}

UINT08 GRAY_MAP_C::GetCostOfLeft(void) const
{
    return cellCostLeftArea_;
}

UINT08 GRAY_MAP_C::GetCostOfRight(void) const
{
    return cellCostRightArea_;
}

/**************************************************************************************
功能描述:  加载地图
           不根据yaml文件中的pgm文件名加载
修改记录:
**************************************************************************************/
int GRAY_MAP_C::LoadMap(const string &strMapPath, bool isExistYamlFile)
{
    /* 存在yaml文件时，读取相关信息，不存在时采用默认参数 */
    if (isExistYamlFile) {
        MAP_YAML_FILE_C objYamlFile;
        string strPgmFileName;      /* 暂时不用这个文件名来读PGM */
        if (0 != objYamlFile.Read(strMapPath, strPgmFileName, imageResolution_, stOriginPos_)) {
            ST_LOG_ERR("Read Yaml File Fail.");
            return -1;
        }
    } else {
        imageResolution_ = MAP_RESOLUTION;
        stOriginPos_.SetZero();
    }

    if (0 != LoadImage(strMapPath)) {
        ST_LOG_ERR("Read Pgm File Fail.");
        return -1;
    }

    ST_LOG_INFO("Load Map(%s) Succ.", strMapPath.c_str());
    return 0;
}

int GRAY_MAP_C::SaveMap(const string &strMapPath) const
{
    if (0 != SaveImage(strMapPath)) {
        ST_LOG_ERR("Save Image File Fail.");
        return -1;
    }

    /* 写YAML文件 */
    MAP_YAML_FILE_C objYamlFile;
    if (0 != objYamlFile.Write(strMapPath, imageResolution_, stOriginPos_)) {
        ST_LOG_ERR("Save Yaml Fail.");
        return -1;
    }

    ST_LOG_INFO("Save %s Succ.", strMapPath.c_str());
    return 0;
}

/**************************************************************************************
功能描述:  创建地图
修改记录:
**************************************************************************************/
INT32 GRAY_MAP_C::CreateMap(const COORDINATE_2D_STRU &stOriginPos, UINT32 mapSizeX, UINT32 mapSizeY, BOOL isDefaultNull, float resolution)
{
    stOriginPos_     = stOriginPos;
    imageResolution_ = resolution;

    UINT08 defaultVal = isDefaultNull ? cellCostNull_ : cellCostUnknown_;
    if (0 != CreateImage(mapSizeX, mapSizeY, defaultVal)) {
        ST_LOG_ERR("CreateImage(%u, %u) Fail.", mapSizeX, mapSizeY);
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述:  创建地图
           根据左下角和右上角坐标确定地图大小和原点坐标
修改记录:
**************************************************************************************/
int GRAY_MAP_C::CreateMap(const COORDINATE_2D_STRU &stLeftButtomPoint, const COORDINATE_2D_STRU &stRightTopPoint, BOOL isDefaultNull, float resolution)
{
    //ST_LOG_INFO("LB.x=%f, LB.y=%f, RT.x=%f, RT.y=%f.", stLeftButtomPoint.x, stLeftButtomPoint.y, stRightTopPoint.x, stRightTopPoint.y);
    imageResolution_ = resolution;

    UINT32 sizeX;
    UINT32 sizeY;
    if (0 != CalcMapInfo(stLeftButtomPoint, stRightTopPoint, stOriginPos_, sizeX, sizeY)) {
        ST_LOG_ERR("Set Map Info Fail.");
        return -1;
    }

    UINT08 defaultVal = isDefaultNull ? cellCostNull_ : cellCostUnknown_;
    if (0 != CreateImage(sizeX, sizeY, defaultVal)) {
        ST_LOG_ERR("CreateImage Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述:  创建地图
修改记录:
**************************************************************************************/
int GRAY_MAP_C::CreateMap(UINT32 mapSizeX, UINT32 mapSizeY, UINT08 *pucImage)
{
    stOriginPos_.SetZero();
    imageResolution_ = MAP_RESOLUTION;

    if (0 != CreateImage(mapSizeX, mapSizeY, pucImage)) {
        ST_LOG_ERR("CreateImage Fail.");
        return -1;
    }

    //ST_LOG_INFO("CreateImage Succ.");
    return 0;
}

/**************************************************************************************
功能描述:  创建地图
修改记录:
**************************************************************************************/
INT32 GRAY_MAP_C::CreateMap(UINT32 mapSizeX, UINT32 mapSizeY, UINT08 defaultVal, float resolution)
{
    stOriginPos_.SetZero();
    imageResolution_ = resolution;

    if (0 != CreateImage(mapSizeX, mapSizeY, defaultVal)) {
        ST_LOG_ERR("CreateImage Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 重置地图范围，与之前重叠部分内容保留
修改记录:
**************************************************************************************/
INT32 GRAY_MAP_C::ResizeMap(COORDINATE_2D_STRU &stNewLeftButtPoint, COORDINATE_2D_STRU &stNewRightTopPoint, BOOL isDefaultNull)
{
    R_ASSERT(IsImageValid(), -1);

    COORDINATE_2D_STRU stNewMapOrigin;
    UINT32             newMapSizeX;
    UINT32             newMapSizeY;
    INT32 rslt0 = CalcMapInfo(stNewLeftButtPoint, stNewRightTopPoint, stNewMapOrigin, newMapSizeX, newMapSizeY);
    R_ASSERT(rslt0 == 0, -1);

    IMAGE_COORDINATE_STRU stNewMapSize = {INT32(newMapSizeX), INT32(newMapSizeY)};
    IMAGE_COORDINATE_STRU stNewLeftBuffCoordByOld;
    ConvPosToMapCoord(&stNewLeftButtPoint, &stNewLeftBuffCoordByOld);

    UINT08 defaultVal = isDefaultNull ? cellCostNull_ : cellCostUnknown_;
    INT32 rslt1 = ResizeImage(stNewMapSize, stNewLeftBuffCoordByOld, defaultVal);
    if (rslt1 != 0) {
        ST_LOG_INFO("ResizeImage() Fail.");
        return -1;
    }

    stOriginPos_.Copy(stNewMapOrigin);
    return 0;
}

INT32 GRAY_MAP_C::CopyMap(const GRAY_MAP_C &objSrcMap, const COORDINATE_2D_STRU &stLeftButtomPoint, const COORDINATE_2D_STRU &stRightTopPoint)
{
    R_ASSERT(objSrcMap.IsValid(), -1);

    DeleteMap();

    IMAGE_COORDINATE_STRU stSrcLeftButtomPointCoord;
    IMAGE_COORDINATE_STRU stSrcRightTopPointCoord;
    objSrcMap.ConvPosToMapCoord(&stLeftButtomPoint, &stSrcLeftButtomPointCoord);
    objSrcMap.ConvPosToMapCoord(&stRightTopPoint, &stSrcRightTopPointCoord);
    IMAGE_COORDINATE_STRU stCopySize;
    stCopySize.x = stSrcRightTopPointCoord.x - stSrcLeftButtomPointCoord.x + 1;
    stCopySize.y = stSrcRightTopPointCoord.y - stSrcLeftButtomPointCoord.y + 1;

    INT32 rslt = CopyImage(objSrcMap, stSrcLeftButtomPointCoord, stCopySize);
    if (rslt != 0) {
        ST_LOG_ERR("CopyImage() Fail.");
        return -1;
    }

    objSrcMap.ConvMapCoordToPos(stSrcLeftButtomPointCoord, stOriginPos_);
    imageResolution_ = objSrcMap.imageResolution_;
    return 0;
}

INT32 GRAY_MAP_C::CopyMap(const GRAY_MAP_C &objSrcMap)
{
    R_ASSERT(objSrcMap.IsValid(), -1);

    DeleteMap();

    INT32 rslt = CopyImage(objSrcMap);
    if (rslt != 0) {
        ST_LOG_ERR("CopyImage() Fail.");
        return -1;
    }

    stOriginPos_.Copy(objSrcMap.stOriginPos_);
    imageResolution_ = objSrcMap.imageResolution_;
    return 0;
}

void GRAY_MAP_C::DeleteMap(void)
{
    FreeImage();
}

void GRAY_MAP_C::SwapMap(GRAY_MAP_C &objOtherMap)
{
    COORDINATE_2D_STRU stOtherOriginPos     = objOtherMap.stOriginPos_;
    float              otherImageResolution = objOtherMap.imageResolution_;

    objOtherMap.stOriginPos_     = this->stOriginPos_;
    objOtherMap.imageResolution_ = this->imageResolution_;

    this->stOriginPos_     = stOtherOriginPos;
    this->imageResolution_ = otherImageResolution;

    SwapImage(objOtherMap);
}

void GRAY_MAP_C::SetCellByPos(const COORDINATE_2D_STRU &stPos, unsigned char setVal)
{
    IMAGE_COORDINATE_STRU stCoord;
    ConvPosToMapCoord(&stPos, &stCoord);

    SetCell(&stCoord, setVal);
}

void GRAY_MAP_C::SetCellsByPos(const list<COORDINATE_2D_STRU> &posLst, unsigned char setVal)
{
    for (list<COORDINATE_2D_STRU>::const_iterator iter = posLst.begin();
         iter != posLst.end();
         iter++) {
        SetCellByPos(*iter, setVal);
    }
}

void GRAY_MAP_C::SetCellsByPos(const vector<COORDINATE_2D_STRU> &vectPos, UINT08 setVal)
{
    for (vector<COORDINATE_2D_STRU>::const_iterator iter = vectPos.begin();
         iter != vectPos.end();
         iter++) {
        SetCellByPos(*iter, setVal);
    }
}

/**************************************************************************************
功能描述: 对四边形外接矩形区域设置对应值
修改记录:
**************************************************************************************/
void GRAY_MAP_C::SetCellsOfRectangleIncludingQuadrangle(const COORDINATE_2D_STRU &stVertice1, const COORDINATE_2D_STRU &stVertice2, const COORDINATE_2D_STRU &stVertice3, const COORDINATE_2D_STRU &stVertice4, UINT08 setVal)
{
    COORDINATE_2D_STRU stLeftButtPoint;
    stLeftButtPoint.x = GetMin(stVertice1.x, stVertice2.x, stVertice3.x, stVertice4.x);
    stLeftButtPoint.y = GetMin(stVertice1.y, stVertice2.y, stVertice3.y, stVertice4.y);

    COORDINATE_2D_STRU stRightTopPoint;
    stRightTopPoint.x = GetMax(stVertice1.x, stVertice2.x, stVertice3.x, stVertice4.x);
    stRightTopPoint.y = GetMax(stVertice1.y, stVertice2.y, stVertice3.y, stVertice4.y);

    IMAGE_COORDINATE_STRU stLeftButtPointCoord;
    ConvPosToMapCoord(&stLeftButtPoint, &stLeftButtPointCoord);

    IMAGE_COORDINATE_STRU stRightTopPointCoord;
    ConvPosToMapCoord(&stRightTopPoint, &stRightTopPointCoord);

    for (INT32 loopOfY = stLeftButtPointCoord.y; loopOfY <= stRightTopPointCoord.y; ++loopOfY) {
        for (INT32 loopOfX = stLeftButtPointCoord.x; loopOfX <= stRightTopPointCoord.x; ++loopOfX) {
            SetCell(loopOfX, loopOfY, setVal);
        }
    }
}

/**************************************************************************************
功能描述: 对线段路径设置对应值
修改记录:
**************************************************************************************/
void GRAY_MAP_C::SetCellsOfLineSegment(const COORDINATE_2D_STRU &stPoint1, const COORDINATE_2D_STRU &stPoint2, UINT08 setVal, vector<IMAGE_COORDINATE_STRU> *pvectSetPoint, BOOL isSetEnd)
{
    vector<IMAGE_COORDINATE_STRU> vctCells;
    GetCellsOfLineSeg(stPoint1, stPoint2, vctCells);

    UINT32 cellsNum = vctCells.size();
    R_ASSERT(cellsNum > 0, NOTHING);
    UINT32 setCellsNum = isSetEnd ? cellsNum : (cellsNum - 1);

    for (UINT32 loop = 0; loop < setCellsNum; ++loop) {
        IMAGE_COORDINATE_STRU &stCell = vctCells[loop];
        if (setVal != GetCellCost(stCell)) {
            SetCell(&stCell, setVal);

            if (pvectSetPoint != NULL) {
                pvectSetPoint->push_back(stCell);
            }
        }
    }
}

/**************************************************************************************
功能描述: 将连续点表示的曲线所在的栅格设置为指定值
修改记录:
**************************************************************************************/
void GRAY_MAP_C::SetCellsOfLine(const vector<COORDINATE_2D_STRU> &vectLine, UINT08 setVal, vector<IMAGE_COORDINATE_STRU> *pvectSetPoint, UINT32 beginIdx, UINT32 endIdx, BOOL isSetEnd)
{
    UINT32 LineSize = UINT32(vectLine.size());
    if (LineSize == 0) {
        return;
    }

    R_ASSERT_2(beginIdx < LineSize, NOTHING, beginIdx, LineSize);

    UINT32 endIdxEx = MIN(endIdx, LineSize - 1);
    for (UINT32 loop = beginIdx + 1; loop <= endIdxEx; ++loop) {
        SetCellsOfLineSegment(vectLine[loop - 1], vectLine[loop], setVal, pvectSetPoint, false);
    }

    if (isSetEnd) {
        const COORDINATE_2D_STRU &stEndPoint = vectLine[endIdxEx];
        IMAGE_COORDINATE_STRU     stEndCell;
        ConvPosToMapCoord(&stEndPoint, &stEndCell);

        SetCell(&stEndCell, setVal);
        if (pvectSetPoint != NULL) {
            pvectSetPoint->push_back(stEndCell);
        }
    }
}

/**************************************************************************************
功能描述: 将与参考位置具有相同像素值的封闭区域设置为指定像素值
修改记录:
**************************************************************************************/
void GRAY_MAP_C::SetCellsOfSameCostArea(const COORDINATE_2D_STRU &stRefPos, UINT08 setVal)
{
    IMAGE_COORDINATE_STRU stRefCell;
    ConvPosToMapCoord(&stRefPos, &stRefCell);

    SetAreaOfSameColor(stRefCell, setVal);
}

/**************************************************************************************
功能描述: 设置给定曲线左右两侧区域
修改记录:
**************************************************************************************/
INT32 GRAY_MAP_C::SetLeftAndRightAreaOfLine(const vector<COORDINATE_2D_STRU> &vectLine, float areaDist, UINT32 splitLineSize)
{
    UINT32 lineSize = UINT32(vectLine.size());
    R_ASSERT_2(lineSize > (splitLineSize * 2), -1, lineSize, splitLineSize);

    SetCellsOfLine(vectLine, cellCostSplitLine);

    vector<IMAGE_COORDINATE_STRU> vectSetPoint;
    SetCellsOfLine(vectLine, cellCostRightArea_, &vectSetPoint, splitLineSize, lineSize - splitLineSize);

    INT32 rslt = SetLeftAndRightPixelsOfLine(vectSetPoint, cellCostLeftArea_, cellCostRightArea_, cellCostSplitLine, GET_CLOSEST_MULTIPLE(areaDist, imageResolution_));
    R_ASSERT(rslt == 0, -1);

    return 0;
}

/**************************************************************************************
功能描述: 设置圆形
修改记录:
**************************************************************************************/
void GRAY_MAP_C::SetCellsOfCircle(const COORDINATE_2D_STRU &stCenterPos, float radius, UINT08 setVal)
{
    R_ASSERT_1(radius > 0, NOTHING, radius);

    float deltaAngle = imageResolution_ / radius;
    for (float angle = 0; angle <= 360.0; angle += deltaAngle) {
        COORDINATE_2D_STRU stPoint;
        stPoint.x = stCenterPos.x + radius * cos(angle);
        stPoint.y = stCenterPos.y + radius * sin(angle);

        SetCellByPos(stPoint, setVal);
    }
}

void GRAY_MAP_C::SetMap(UINT08 setVal)
{
    R_ASSERT(IsValid(), NOTHING);

    SetImage(setVal);
}

void GRAY_MAP_C::SetMap(const COORDINATE_2D_STRU &stLeftButtPoint, UINT32 mapSizeX, UINT32 mapSizeY, UINT08 *pucMap)
{
    R_ASSERT(IsValid(), NOTHING);

    IMAGE_COORDINATE_STRU stLeftButtCoord;
    ConvPosToMapCoord(&stLeftButtPoint, &stLeftButtCoord);

    SetImage(stLeftButtCoord, mapSizeX, mapSizeY, pucMap);
}

void GRAY_MAP_C::SetMapNull(void)
{
    R_ASSERT(IsValid(), NOTHING);

    SetImage(cellCostNull_);
}

void GRAY_MAP_C::SetOrigin(const COORDINATE_2D_STRU &stOrigin)
{
    stOriginPos_.Copy(stOrigin);
}

BOOL GRAY_MAP_C::IsCellsOfLinSegAllNull(const COORDINATE_2D_STRU &stPoint1, const COORDINATE_2D_STRU &stPoint2) const
{
    vector<IMAGE_COORDINATE_STRU> vctCells;
    GetCellsOfLineSeg(stPoint1, stPoint2, vctCells);

    for (UINT32 loop = 0; loop < vctCells.size(); ++loop) {
        IMAGE_COORDINATE_STRU &stCell = vctCells[loop];
        UINT32 cellCost = GetCellCost(stCell);

        if (cellCostNull_ != cellCost) {
            //ST_LOG_ERR("CellCost = %u. x-y(%u, %u).", cellCost, stCell.x, stCell.y);
            return false;
        }
    }

    return true;
}

UINT08 GRAY_MAP_C::GetCellByPos(const COORDINATE_2D_STRU &stPos) const
{
    IMAGE_COORDINATE_STRU stCoord;
    ConvPosToMapCoord(&stPos, &stCoord);

    return GetCellCost(stCoord);
}

INT32 GRAY_MAP_C::GetOverlapArea(const AREA_C &objSrcArea, AREA_C &objOverlapArea) const
{
    R_ASSERT(IsValid(), -1);

    COORDINATE_2D_STRU stMapRTPoint;
    GetRTPoint(stMapRTPoint);

    AREA_C objMapArea;
    objMapArea.IncludePoint(stOriginPos_);
    objMapArea.IncludePoint(stMapRTPoint);

    objSrcArea.GetOverlapArea(objMapArea, objOverlapArea);
    return 0;
}

INT32 GRAY_MAP_C::GetCellsPosListInArea(const AREA_C &objArea, UINT08 cellCost, vector<COORDINATE_2D_STRU> &vectCellsPosList) const
{
    AREA_C objOverlapArea;
    INT32 rslt = GetOverlapArea(objArea, objOverlapArea);
    R_ASSERT(rslt == 0, -1);
    if (!objOverlapArea.IsValid()) {
        return 0;
    }

    IMAGE_COORDINATE_STRU stAreaLBCoord;
    ConvPosToMapCoord(&objOverlapArea.stLBPoint, &stAreaLBCoord);
    IMAGE_COORDINATE_STRU stAreaRTCoord;
    ConvPosToMapCoord(&objOverlapArea.stRTPoint, &stAreaRTCoord);
    IMAGE_COORDINATE_STRU stAreaSize;
    //stAreaSize.x = stAreaRTCoord.x - stAreaLBCoord.x + 1;
    //stAreaSize.y = stAreaRTCoord.y - stAreaLBCoord.y + 1;
    stAreaSize.x = stAreaRTCoord.x - stAreaLBCoord.x + 1;            /* 注意，此处的意思是不能处理右和上边界 */
    stAreaSize.y = stAreaRTCoord.y - stAreaLBCoord.y + 1;

    vector<IMAGE_COORDINATE_STRU> vectCellCoordList;
    rslt = GetCellsInArea(cellCost, stAreaLBCoord, stAreaSize, vectCellCoordList);
    R_ASSERT(rslt == 0, -1);

    for (UINT32 loop = 0; loop < UINT32(vectCellCoordList.size()); ++loop) {
        IMAGE_COORDINATE_STRU &stCellCoord = vectCellCoordList[loop];
        COORDINATE_2D_STRU     stCellPos;
        ConvMapCoordToPos(stCellCoord, stCellPos);

        vectCellsPosList.push_back(stCellPos);      /* 增量的，不能清除老的信息 */
    }

    return 0;
}

/**************************************************************************************
功能描述: 复制地图(部分)内容
修改记录:
**************************************************************************************/
int GRAY_MAP_C::Merge(COORDINATE_2D_STRU &stTgtLeftButtomPoint, GRAY_MAP_C &objSrc, COORDINATE_2D_STRU &stSrcLeftButtomPoint, COORDINATE_2D_STRU &stSrcRightTopPoint)
{
    IMAGE_COORDINATE_STRU stSrcLeftButtomPointCoord;
    IMAGE_COORDINATE_STRU stSrcRightTopPointCoord;
    objSrc.ConvPosToMapCoord(&stSrcLeftButtomPoint, &stSrcLeftButtomPointCoord);
    objSrc.ConvPosToMapCoord(&stSrcRightTopPoint, &stSrcRightTopPointCoord);
    ST_LOG_INFO("Src LB.x=%d:%f, LB.y=%d:%f, RT.x=%d:%f, RT.y=%d:%f.", stSrcLeftButtomPointCoord.x, stSrcLeftButtomPoint.x, stSrcLeftButtomPointCoord.y, stSrcLeftButtomPoint.y, stSrcRightTopPointCoord.x, stSrcRightTopPoint.x, stSrcRightTopPointCoord.y, stSrcRightTopPoint.y);
    if (!objSrc.isCoordValid(stSrcLeftButtomPointCoord) || !objSrc.isCoordValid(stSrcRightTopPointCoord)) {
        ST_LOG_ERR("Src Point Invalid.");
        return -1;
    }

    IMAGE_COORDINATE_STRU stTgtLeftButtonPointCoord;
    ConvPosToMapCoord(&stTgtLeftButtomPoint, &stTgtLeftButtonPointCoord);
    ST_LOG_INFO("Tgt LB.x=%d, LB.y=%d.", stTgtLeftButtonPointCoord.x, stTgtLeftButtonPointCoord.y);
    if (!isCoordValid(stTgtLeftButtonPointCoord)) {
        ST_LOG_ERR("tgt Left Buttom Point Invalid.");
        return -1;
    }

    int copyXLen = stSrcRightTopPointCoord.x + 1 - stSrcLeftButtomPointCoord.x;
    int copyYLen = stSrcRightTopPointCoord.y + 1 - stSrcLeftButtomPointCoord.y;

    IMAGE_COORDINATE_STRU stTgtRightTopPointCoord;
    stTgtRightTopPointCoord.x = stTgtLeftButtonPointCoord.x + copyXLen - 1;
    stTgtRightTopPointCoord.y = stTgtLeftButtonPointCoord.y + copyYLen - 1;
    ST_LOG_INFO("Tgt RT.x=%d, RT.y=%d.", stTgtRightTopPointCoord.x, stTgtRightTopPointCoord.y);
    if (!isCoordValid(stTgtRightTopPointCoord)) {
        ST_LOG_ERR("tgt Right Top Point(%d, %d) Invalid.", stTgtRightTopPointCoord.x, stTgtRightTopPointCoord.y);
        return -1;
    }

    for (int loopOfY = 0; loopOfY < copyYLen; loopOfY++) {
        for (int loopOfX = 0; loopOfX < copyXLen; loopOfX++) {
            int tgtX = stTgtLeftButtonPointCoord.x + loopOfX;
            int tgtY = stTgtLeftButtonPointCoord.y + loopOfY;
            int srcX = stSrcLeftButtomPointCoord.x + loopOfX;
            int srcY = stSrcLeftButtomPointCoord.y + loopOfY;

            UINT08 *pcTgtCell = GetCellPtr(tgtX, tgtY);
            if (pcTgtCell == NULL) {
                ST_LOG_ERR("Get Tgt Cell(%d, %d) Fail.", tgtX, tgtY);
                return -1;
            }

            UINT08 srcCellCost = objSrc.GetCellCost(srcX, srcY);
#if 0
#if 1
            /* 如果源为奇怪的字符，需要进行修正 */
            if ((srcCellCost != cellCostUnknown_) && (srcCellCost != cellCostNull_) && (srcCellCost != cellCostObst_)) {
                /* 则设置为未知 */
                //srcCellCost = cellCostUnknown_;

                /* 根据周围的值来修正，靠近周围取值 */
                srcCellCost = GetAbnormalCellCorrect(srcCellCost, srcX, srcY);
            }
#else
            /* 如果孤立未知点进行修正修正 */
            if (srcCellCost == cellCostUnknown_) {
                bool isNearExistUnknown;
                bool isNearExistNull;
                bool isNearExistObst;
                objSrc.GetNearCellInfo(srcX, srcY, isNearExistUnknown, isNearExistNull, isNearExistObst);
                if (!isNearExistUnknown) {
                    if (isNearExistNull) {
                        srcCellCost = cellCostNull_;
                    } else {
                        srcCellCost = cellCostObst_;
                    }
                }
            }
#endif
#endif
            //ST_LOG_INFO("tgt x=%d, y=%d, v=%u.", tgtX, tgtY, *pcTgtCell);

            //if        (*pcTgtCell == cellCostUnknown_) {                                        /* 目标未知，必然覆盖 */
            //    *pcTgtCell = *pcSrcCell;
            //} else if (*pcTgtCell == cellCostObst_) {                                           /* 目标为障碍点，如果原点为空白点且目标点为孤立点，则覆盖，否则不覆盖 */
            //    if ((*pcSrcCell == cellCostNull_) && (IsObstOutlier(tgtX, tgtY, 2))) {
            //        *pcTgtCell = *pcSrcCell;
            //    }
            //} else if (*pcTgtCell == cellCostNull_) {                                           /* 目标点为空白点，如果原点为障碍点且不是孤立点，则覆盖，否则不覆盖 */
            //    if ((*pcSrcCell == cellCostObst_) && !objSrc.IsObstOutlier(srcX, srcY, 2)) {
            //        *pcTgtCell = *pcSrcCell;
            //    }
            //} else {                                                                            /* 默认覆盖 */
            //    *pcTgtCell = *pcSrcCell;
            //}
#if 0
            /* 覆盖优先级: 障碍 > 空白 > 未知 */
            if        (*pcTgtCell == cellCostUnknown_) {
                *pcTgtCell = srcCellCost;
            } else if (*pcTgtCell == cellCostNull_) {
                if (srcCellCost == cellCostObst_) {
                    *pcTgtCell = srcCellCost;
                }
            } else if (*pcTgtCell == cellCostObst_) {
            } else {
                *pcTgtCell = srcCellCost;
            }
#else
            if (*pcTgtCell != cellCostVirtualObst_) {
                *pcTgtCell = srcCellCost;
            }
#endif
        }
    }

    return 0;
}

/**************************************************************************************
功能描述: 获取地图有效内容的区域
          地图区域采用左下角和右上角表示
修改记录:
**************************************************************************************/
int GRAY_MAP_C::GetEffectArea(COORDINATE_2D_STRU &stLeftButtomPoint, COORDINATE_2D_STRU &stRightTopPoint)
{
    R_ASSERT(IsImageValid(), -1);

    int mapSizeX = (int)GetImageSizeX();                                        /* 规避告警，防止有符号和无符号数一起比较 */
    int mapSizeY = (int)GetImageSizeY();

    /* 最上边的点 *//* 注意行号越大，在地图的越上面 */
    IMAGE_COORDINATE_STRU stTopPointCoord;
    bool isExistEffectCell = false;
    for (stTopPointCoord.y = mapSizeY - 1; stTopPointCoord.y >= 0; --stTopPointCoord.y) {
        for (stTopPointCoord.x = 0; stTopPointCoord.x < mapSizeX; ++stTopPointCoord.x) {
            if (IsCellEffect(stTopPointCoord.x, stTopPointCoord.y)) {
                isExistEffectCell = true;
                break;
            }
        }

        if (isExistEffectCell) {
            ST_LOG_INFO("T.x=%d, T.y=%d.", stTopPointCoord.x, stTopPointCoord.y);
            break;
        }
    }

    if (!isExistEffectCell) {
        ST_LOG_ERR("No Effect Point.");
        return -1;
    }



    /* 最左边的点 */
    IMAGE_COORDINATE_STRU stLeftPointCoord;
    isExistEffectCell = false;
    for (stLeftPointCoord.x = 0; stLeftPointCoord.x < mapSizeX; ++stLeftPointCoord.x) {
        for (stLeftPointCoord.y = stTopPointCoord.y; stLeftPointCoord.y >= 0; --stLeftPointCoord.y) {
            if (IsCellEffect(stLeftPointCoord.x, stLeftPointCoord.y)) {
                isExistEffectCell = true;
                break;
            }
        }

        if (isExistEffectCell) {
            ST_LOG_INFO("L.x=%d, L.y=%d.", stLeftPointCoord.x, stLeftPointCoord.y);
            break;
        }
    }


    /* 最下边的点 */
    IMAGE_COORDINATE_STRU stButtomPointCoord;
    isExistEffectCell = false;
    for (stButtomPointCoord.y = 0; stButtomPointCoord.y < mapSizeY; ++stButtomPointCoord.y) {
        for (stButtomPointCoord.x = mapSizeX - 1; stButtomPointCoord.x >= stLeftPointCoord.x; --stButtomPointCoord.x) {
            if (IsCellEffect(stButtomPointCoord.x, stButtomPointCoord.y)) {
                isExistEffectCell = true;
                break;
            }
        }

        if (isExistEffectCell) {
            ST_LOG_INFO("B.x=%d, B.y=%d.", stButtomPointCoord.x, stButtomPointCoord.y);
            break;
        }
    }

    /* 最右边的点 */
    IMAGE_COORDINATE_STRU stRightPointCoord;
    isExistEffectCell = false;
    for (stRightPointCoord.x = mapSizeX - 1; stRightPointCoord.x >= 0; --stRightPointCoord.x) {
        for (stRightPointCoord.y = stButtomPointCoord.y; stRightPointCoord.y <= stTopPointCoord.y; ++stRightPointCoord.y) {
            if (IsCellEffect(stRightPointCoord.x, stRightPointCoord.y)) {
                isExistEffectCell = true;
                break;
            }
        }

        if (isExistEffectCell) {
            ST_LOG_INFO("R.x=%d, R.y=%d.", stRightPointCoord.x, stRightPointCoord.y);
            break;
        }
    }

    IMAGE_COORDINATE_STRU stLeftButtomPointCoord = {stLeftPointCoord.x, stButtomPointCoord.y};
    ConvMapCoordToPos(stLeftButtomPointCoord, stLeftButtomPoint);

    IMAGE_COORDINATE_STRU stRightTopPointCoord = {stRightPointCoord.x, stTopPointCoord.y};
    ConvMapCoordToPos(stRightTopPointCoord, stRightTopPoint);

    ST_LOG_INFO("LB.x=%d:%f, LB.y=%d:%f, RT.x=%d:%f, RT.y=%d:%f.",
            stLeftButtomPointCoord.x, stLeftButtomPoint.x,
            stLeftButtomPointCoord.y, stLeftButtomPoint.y,
            stRightTopPointCoord.x, stRightTopPoint.x,
            stRightTopPointCoord.y, stRightTopPoint.y);
    return 0;
}

/**************************************************************************************
功能描述: 过滤孤立障碍点
修改记录:
**************************************************************************************/
void GRAY_MAP_C::FilterObstOutlier(UINT32 filterRange, UINT32 filterLowerCount)
{
    INT32 mapSizeX = (INT32)GetImageSizeX();
    INT32 mapSizeY = (INT32)GetImageSizeY();

    for (int loopOfX = 0; loopOfX < (int)mapSizeX; ++loopOfX) {
        for (int loopOfY = 0; loopOfY < (int)mapSizeY; ++loopOfY) {
            if (cellCostObst_ != GetCellCost(loopOfX, loopOfY)) {
                continue;
            }

            if (IsObstOutlier(loopOfX, loopOfY, filterRange, filterLowerCount)) {
                SetCell(loopOfX, loopOfY, cellCostNull_);
            }
        }
    }
}

UINT08 GRAY_MAP_C::GetCellCost(int cellX, int cellY) const
{
    UINT08 cellCost;

    if (0 != GetCell(cellX, cellY, cellCost)) {
        return cellCostOver_;
    }

    return cellCost;
}

UINT08 GRAY_MAP_C::GetCellCost(const IMAGE_COORDINATE_STRU &stCellCoord) const
{
    return GetCellCost(stCellCoord.x, stCellCoord.y);
}

void GRAY_MAP_C::GetRTPoint(COORDINATE_2D_STRU &stRTPoint) const
{
    IMAGE_COORDINATE_STRU stRTCoord;
    GetRTCoord(stRTCoord);

    ConvMapCoordToPos(stRTCoord, stRTPoint);
}

/**************************************************************************************
功能描述: 获取指定两点的线段对应的像素列表
          从起点到终点有序排列，含起点和终点
修改记录:
**************************************************************************************/
void GRAY_MAP_C::GetCellsOfLineSeg(const COORDINATE_2D_STRU &stPoint1, const COORDINATE_2D_STRU &stPoint2, vector<IMAGE_COORDINATE_STRU> &vctCells) const
{
    IMAGE_COORDINATE_STRU stPoint1Coord;
    ConvPosToMapCoord(&stPoint1, &stPoint1Coord);
    vctCells.push_back(stPoint1Coord);

    IMAGE_COORDINATE_STRU stPoint2Coord;
    ConvPosToMapCoord(&stPoint2, &stPoint2Coord);

    INT32 deltaCoordX    = stPoint2Coord.x - stPoint1Coord.x;
    INT32 deltaCoordY    = stPoint2Coord.y - stPoint1Coord.y;
    INT32 deltaCoordXAbs = ABS(deltaCoordX);
    INT32 deltaCoordYAbs = ABS(deltaCoordY);
    float incX;
    float incY;

    if (deltaCoordXAbs >= deltaCoordYAbs) {
        incX = 1.0;
        incY = float(deltaCoordYAbs) / float(deltaCoordXAbs);
    } else {
        incX = float(deltaCoordXAbs) / float(deltaCoordYAbs);
        incY = 1.0;
    }

    INT32 signIncX   = SIGN(deltaCoordX);
    INT32 signIncY   = SIGN(deltaCoordY);
    float deltaX     = 0;
    float deltaY     = 0;
    INT32 imageDistX = 0;
    INT32 imageDistY = 0;

    INT32 loopMax    = MAX(deltaCoordXAbs, deltaCoordYAbs);
    for (INT32 loop = 0; loop < loopMax; ++loop) {
        deltaX += incX;
        if (deltaX >= 1.0) {
            ++imageDistX;
            deltaX -= 1.0;
        }

        deltaY += incY;
        if (deltaY >= 1.0) {
            ++imageDistY;
            deltaY -= 1.0;
        }

        IMAGE_COORDINATE_STRU stPointCoord;
        stPointCoord.x = stPoint1Coord.x + (imageDistX + ROUND_UP_DOWN(deltaX)) * signIncX;
        stPointCoord.y = stPoint1Coord.y + (imageDistY + ROUND_UP_DOWN(deltaY)) * signIncY;
        vctCells.push_back(stPointCoord);
    }
}

void GRAY_MAP_C::ConvPosToMapCoord(const COORDINATE_2D_STRU *pstOriginPos, const COORDINATE_2D_STRU *pstPos, IMAGE_COORDINATE_STRU *pstCoord) const
{
    pstCoord->x = (int)((pstPos->x - pstOriginPos->x) / imageResolution_ + 0.5);
    pstCoord->y = (int)((pstPos->y - pstOriginPos->y) / imageResolution_ + 0.5);
}

void GRAY_MAP_C::ConvPosToMapCoord(const COORDINATE_2D_STRU *pstPos, IMAGE_COORDINATE_STRU *pstCoord) const
{
    ConvPosToMapCoord(&stOriginPos_, pstPos, pstCoord);
}

void GRAY_MAP_C::ConvMapCoordToPos(const IMAGE_COORDINATE_STRU &stCoord, COORDINATE_2D_STRU &stPos) const
{
    stPos.x = stCoord.x * imageResolution_ + stOriginPos_.x;
    stPos.y = stCoord.y * imageResolution_ + stOriginPos_.y;
}

float GRAY_MAP_C::CalcDistByCoordDiff(int diffX, int diffY)
{
    return hypot((float)diffX, (float)diffY) * imageResolution_;
}

unsigned int GRAY_MAP_C::GetCoorDist(float dist)
{
    return (unsigned int)(dist / imageResolution_);
}

int GRAY_MAP_C::Calc2CellDistSqu(int cell1X, int cell1Y, int cell2X, int cell2Y)
{
    int diffX = cell1X - cell2X;
    int diffY = cell1Y - cell2Y;
    return (diffX * diffX + diffY * diffY);
}

int GRAY_MAP_C::CalcMapInfo(const COORDINATE_2D_STRU &stLeftButtomPoint, const COORDINATE_2D_STRU &stRightTopPoint, COORDINATE_2D_STRU &stOriginPos, UINT32 &sizeX, UINT32 &sizeY)
{
    if ((stLeftButtomPoint.x >= stRightTopPoint.x) || (stLeftButtomPoint.y >= stRightTopPoint.y)) {
        ST_LOG_ERR("left buttom is larger than right top point coord.");
        return -1;
    }

    stOriginPos.Copy(stLeftButtomPoint);

    IMAGE_COORDINATE_STRU stRightTopCoord;
    ConvPosToMapCoord(&stOriginPos, &stRightTopPoint, &stRightTopCoord);
    sizeX = (UINT32)(stRightTopCoord.x + 1);
    sizeY = (UINT32)(stRightTopCoord.y + 1);

    return 0;
}

/**************************************************************************************
功能描述: 判断指定栅格是否有有效
          通行或者障碍，则为有效
修改记录:
**************************************************************************************/
bool GRAY_MAP_C::IsCellEffect(int cellX, int cellY)
{
    UINT08 cellCost = GetCellCost(cellX, cellY);

    return ((cellCost == cellCostObst_) || (cellCost == cellCostNull_));
}

/**************************************************************************************
功能描述: 判断指定栅格是否为孤立点
修改记录:
**************************************************************************************/
bool GRAY_MAP_C::IsObstOutlier(int cellX, int cellY, int range, int judgeLowerCount)
{
    int obstCellCount = 1;

    for (int loopOfX = -1 * range; loopOfX <= range; ++loopOfX) {
        for (int loopOfY = -1 * range; loopOfY <= range; ++loopOfY) {
            if ((loopOfX == 0) && (loopOfY == 0)) {
                continue;
            }

            UINT08 cellCostOther = GetCellCost(cellX + loopOfX, cellY + loopOfY);

            /* 如果存在非空非障碍点，则认为不是障碍孤立点，当前只处理被空白包围的障碍点 */
            if ((cellCostOther != cellCostNull_) && (cellCostOther != cellCostObst_)) {
                return false;
            }

            if (cellCostOther == cellCostObst_) {
                obstCellCount++;
            }
        }
    }

    //ST_LOG_ERR("x=%d, y=%d, limit=%d, count=%d.", cellX, cellY, judgeLowerCount, obstCellCount);

    /* 障碍点少于指定个数，则认为时孤立点 */
    return (obstCellCount <= judgeLowerCount);
}

/**************************************************************************************
功能描述: 获取指定点周围信息
修改记录:
**************************************************************************************/
void GRAY_MAP_C::GetNearCellInfo(int cellX, int cellY, bool &isNearExistUnknown, bool &isNearExistNull, bool &isNearExistObst)
{
    isNearExistUnknown = false;
    isNearExistNull = false;
    isNearExistObst = false;

    for (int loopOfY = -1; loopOfY <= 1; loopOfY++) {
        for (int loopOfX = -1; loopOfX <= 1; loopOfX++) {
            if ((loopOfX == 0) && (loopOfY == 0)) {
                continue;
            }

            UINT08 cellCostOther = GetCellCost(cellX + loopOfX, cellY + loopOfY);
            if (cellCostOther == cellCostNull_) {
                isNearExistNull = true;
            } else if (cellCostOther == cellCostObst_) {
                isNearExistObst = true;
            } else if (cellCostOther == cellCostUnknown_) {
                isNearExistUnknown = true;
            } else {
            }
        }
    }
}

/**************************************************************************************
功能描述: 获取异常点修正值
修改记录:
**************************************************************************************/
UINT08 GRAY_MAP_C::GetAbnormalCellCorrect(UINT08 cellCost, int cellX, int cellY)
{
    UINT08 nearObstNull = GET_NEAR_VAL(cellCost, cellCostObst_, cellCostNull_);
    UINT08 nearObstUnkn = GET_NEAR_VAL(cellCost, cellCostObst_, cellCostUnknown_);
    UINT08 nearNullUnkn = GET_NEAR_VAL(cellCost, cellCostNull_, cellCostUnknown_);
    if        ((nearObstNull == cellCostObst_) && (nearObstUnkn == cellCostObst_)) {
        return cellCostObst_;
    } else if ((nearObstNull == cellCostNull_) && (nearNullUnkn == cellCostNull_)) {
        return cellCostNull_;
    } else {
    }

    bool isNearExistUnknown;
    bool isNearExistNull;
    bool isNearExistObst;
    GetNearCellInfo(cellX, cellY, isNearExistUnknown, isNearExistNull, isNearExistObst);


    /* 周围都是一样的，则和周围一样 */
    if        (isNearExistUnknown && !isNearExistNull && !isNearExistObst) {
        return cellCostUnknown_;
    } else if (!isNearExistUnknown && isNearExistNull && !isNearExistObst) {
        return cellCostNull_;
    } else if (!isNearExistUnknown && !isNearExistNull && isNearExistObst) {
        return cellCostObst_;
    /* 周围有空和障碍，则只能时空和障碍 */
    } else if (isNearExistNull && isNearExistObst) {
        return nearObstNull;
    /* 其它只有两样的，取进的 */
    } else if (isNearExistUnknown && isNearExistNull && !isNearExistObst) {
        return nearNullUnkn;
    } else if (isNearExistUnknown && !isNearExistNull && isNearExistObst) {
        return nearObstUnkn;
    /* 都不存在的，未知 */
    } else {
        return cellCostUnknown_;
    }
}

BOOL TRC_MAP_C::IsCellObst(int cellX, int cellY) const
{
    return (GetCellCost(cellX, cellY) == cellCostObst_);
}


INT32 TRC_MAP_C::Reset(const COORDINATE_2D_STRU &stRouteAreaLeftButtPoint, const COORDINATE_2D_STRU &stRouteAreaRightTopPoint)
{
    COORDINATE_2D_STRU stLeftButtPoint;
    stLeftButtPoint.x = stRouteAreaLeftButtPoint.x - 10.0;
    stLeftButtPoint.y = stRouteAreaLeftButtPoint.y - 10.0;

    COORDINATE_2D_STRU stRightTopPoint;
    stRightTopPoint.x = stRouteAreaRightTopPoint.x + 10.0;
    stRightTopPoint.y = stRouteAreaRightTopPoint.y + 10.0;

    INT32 rslt = CreateMap(stLeftButtPoint, stRightTopPoint, true);
    if (rslt != 0) {
        ST_LOG_ERR("CreateMap(lb.x=%f, .y=%f, rt.x=%f, .y=%f.) Fail.", stRouteAreaLeftButtPoint.x, stRouteAreaLeftButtPoint.y, stRouteAreaRightTopPoint.x, stRouteAreaRightTopPoint.y);
        return -1;
    }

    stGoForwardTrcPoint_.clear();

    return 0;
}

BOOL TRC_MAP_C::Is4SideAreaPassable(const COORDINATE_2D_STRU &stLeftFront, const COORDINATE_2D_STRU &stRightFront, const COORDINATE_2D_STRU &stLeftBack, const COORDINATE_2D_STRU &stRightBack) const
{
    /* 判断车体四边 */
    return (   IsCellsOfLinSegAllNull(stLeftFront,  stRightFront)
            && IsCellsOfLinSegAllNull(stLeftBack,   stRightBack)
            && IsCellsOfLinSegAllNull(stLeftFront,  stLeftBack)
            && IsCellsOfLinSegAllNull(stRightFront, stRightBack));
}

/**************************************************************************************
功能描述: 判断车体区域的所有栅格是否可通行
修改记录:
**************************************************************************************/
BOOL TRC_MAP_C::IsAllAreaPassable(const COORDINATE_2D_STRU &stLeftFront, const COORDINATE_2D_STRU &stRightFront, const COORDINATE_2D_STRU &stLeftBack, const COORDINATE_2D_STRU &stRightBack) const
{
    /* 判断车体四边 */
    vector<IMAGE_COORDINATE_STRU> vctCells;
    GetCellsOfLineSeg(stLeftFront,  stRightFront, vctCells);
    GetCellsOfLineSeg(stLeftBack,   stRightBack, vctCells);
    GetCellsOfLineSeg(stLeftFront,  stLeftBack, vctCells);
    GetCellsOfLineSeg(stRightFront, stRightBack, vctCells);

    UINT32 minX = std::numeric_limits<UINT32>::max(), minY = std::numeric_limits<UINT32>::max(), maxX = 0, maxY = 0;
    for(const auto point : vctCells) {
        if(UINT32(point.x) < minX) {
            minX = UINT32(point.x);
        }

        if(UINT32(point.y) < minY) {
            minY = UINT32(point.y);
        }

        if(UINT32(point.x) > maxX) {
            maxX = UINT32(point.x);
        }

        if(UINT32(point.y) > maxY) {
            maxY = UINT32(point.y);
        }

    }

    UINT32 sizeX  = maxX - minX + 1;
    UINT32 sizeY  = maxY - minY + 1;
    UINT32 size   = MAX(sizeX, sizeY);

    vector<BOOL> space(size * size, false);
    for(const auto point : vctCells) {
        UINT32 x = point.x - minX;
        UINT32 y = point.y - minY;
        space[y * size + x] = true;
    }

    // FILL THE SHAPE
    for (UINT32 i = 0; i < size; ++i) {
        // set inside to false
        BOOL inside = false;
        UINT32 hcross1 =0, hcross2 = 0;

        // determine x crossings
        for (UINT32 k = 0; k < size; ++k) {
            if (space[i * size + k] && !inside) {
                hcross1 = k;
                inside = true;
            }

            if (space[i * size + k] && inside) {
                hcross2 = k;
            }
        }

        for (UINT32 j = 0; j < size; ++j) {
            // if inside fill
            if (j > hcross1 && j < hcross2 && inside) {
                space[i * size + j] = true;
            }
        }
    }

    // FILL THE SHAPE
    for (UINT32 i = 0; i < size; ++i) {
        for (UINT32 j = 0; j < size; ++j) {
            if(space[i * size + j]) {
                IMAGE_COORDINATE_STRU stCell{INT32(j + minX), INT32(i + minY)};
                UINT32 cellCost = GetCellCost(stCell);

                if (cellCostNull_ != cellCost) {
                    return false;
                }
            }
        }
    }

    return true;
}

void TRC_MAP_C::SetCarBodyArea(const COORDINATE_2D_STRU &stLeftFront, const COORDINATE_2D_STRU &stRightFront, const COORDINATE_2D_STRU &stLeftBack, const COORDINATE_2D_STRU &stRightBack, BOOL isNeedClr)
{
    vector<IMAGE_COORDINATE_STRU> *pvectRecord = isNeedClr ? &stGoForwardTrcPoint_ : NULL;

    /* 只需要将四边设置，车体移动时自然后将所有轨迹设置 */
    SetCellsOfLineSegment(stLeftFront,  stRightFront, cellCostCarBodyTrc_, pvectRecord);
    SetCellsOfLineSegment(stLeftBack,   stRightBack,  cellCostCarBodyTrc_, pvectRecord);
    SetCellsOfLineSegment(stLeftFront,  stLeftBack,   cellCostCarBodyTrc_, pvectRecord);
    SetCellsOfLineSegment(stRightFront, stRightBack,  cellCostCarBodyTrc_, pvectRecord);
}

void TRC_MAP_C::ClearCarBodyArea(void)
{
    for (UINT32 loop = 0; loop < stGoForwardTrcPoint_.size(); ++loop) {
        IMAGE_COORDINATE_STRU &stCellCoord = stGoForwardTrcPoint_[loop];
        SetCell(&stCellCoord, cellCostNull_);
    }

    stGoForwardTrcPoint_.clear();
}

void TRC_MAP_C::SaveTrcMap(const string &strMapPath)
{
    SaveMap(strMapPath);
}

BOOL TRC_MAP_C::IsTrcMapValid(void) const
{
    return IsValid();
}

BOOL TRC_MAP_C::IsInTrcArea(const COORDINATE_2D_STRU &stPoint) const
{
    IMAGE_COORDINATE_STRU stPointCoord;
    ConvPosToMapCoord(&stPoint, &stPointCoord);

    return (cellCostCarBodyTrc_ == GetCellCost(stPointCoord));
}

#endif


